#include "radar_protocol.h"
#include "platform.h"
#include "log.h"
#include "event.h"

extern radar_manager_t g_radar_manager;
extern event_manager_t g_event_manager;

uint8_t check_sum(char *buffer, int length)
{
    uint8_t sum = 0;
    for (int i = 0; i < length; i++) {
        sum += buffer[i];
    }
    return sum;
}

static uint8_t convert_sstk_to_platform(uint8_t sstk_type)
{
    switch (sstk_type) {
        case 0:
            return 1; /* 小型车 */
        case 1:
            return 7; /* 行人 */
        case 2:
            return 5; /* 自行车 (非机动车) */
        case 3:
            return 2; /* 中型车 */
        case 4:
            return 3; /* 大型车 */
        default:
            return 0; /* 点目标 兜底 */
    }
}

#define BUFFER_SIZE 2048
#define PACKET_SIZE 1024

typedef struct {
    uint8_t data[BUFFER_SIZE];
    int head;
    int tail;
} CircularBuffer;

CircularBuffer cb = {0};

static int buffer_available(const CircularBuffer *cb)
{
    return (cb->head - cb->tail + BUFFER_SIZE) % BUFFER_SIZE;
}

static void buffer_push(CircularBuffer *cb, const uint8_t *data, int len)
{
    for (int i = 0; i < len; i++) {
        cb->data[cb->head] = data[i];
        cb->head = (cb->head + 1) % BUFFER_SIZE;

        // 防止覆盖旧数据
        if (cb->head == cb->tail) {
            cb->tail = (cb->tail + 1) % BUFFER_SIZE;
        }
    }
}

// 获取连续数据（考虑环绕）
static uint8_t buffer_get(const CircularBuffer *cb, int index)
{
    return cb->data[(cb->tail + index) % BUFFER_SIZE];
}

static uint16_t read_uint16_be(CircularBuffer *cb, int offset)
{
    uint8_t hi = buffer_get(cb, offset);
    uint8_t lo = buffer_get(cb, offset + 1);
    return ((uint16_t)hi << 8) | lo;
}

// 校验数据包是否正确
static uint8_t *buffer_parse(CircularBuffer *cb, int *out_len)
{
    int avail = buffer_available(cb);
    int i = 0;

    while (avail - i >= 5) {  // 至少要有帧头+长度+校验1字节
        if (buffer_get(cb, i) == 0xAB && buffer_get(cb, i + 1) == 0xCD) {
            // 长度字段：大端
            uint16_t pkt_len = read_uint16_be(cb, i + 2);  // 数据体长度
            size_t full_len = pkt_len + 5;                 // 2帧头 + 2长度 + N数据 + 1校验

            if (avail - i < full_len) {
                // 数据还不够完整一帧
                break;
            }

            if (full_len > PACKET_SIZE) {
                fprintf(stderr, "Frame too long, discard\n");
                cb->tail = (cb->tail + i + 1) % BUFFER_SIZE;
                return NULL;
            }

            static uint8_t temp_pkt[PACKET_SIZE];
            for (int k = 0; k < full_len; k++) {
                temp_pkt[k] = buffer_get(cb, i + k);
            }

            // 校验：从数据体起点 temp_pkt[4]，共 pkt_len 字节
            uint8_t sum = check_sum(temp_pkt + 4, pkt_len);
            if (sum != temp_pkt[4 + pkt_len]) {
                printf("Checksum error: expect 0x%02X, got 0x%02X\n", sum, temp_pkt[4 + pkt_len]);
                cb->tail = (cb->tail + i + 1) % BUFFER_SIZE;
                return NULL;
            }

            // 校验通过
            cb->tail = (cb->tail + i + full_len) % BUFFER_SIZE;
            *out_len = full_len;
            return temp_pkt;
        }

        i++;
    }

    return NULL;
}

void fix_endian_sstk_header(sstk_header_t *header)
{
    SWAP16(header->length);
    SWAP16(header->type);
}

void fix_endian_sstk_jb(sstk_jb_t *jb)
{
    SWAP16(jb->ll_report_interval);
    SWAP16(jb->protocol_version);
}

void fix_endian_sstk_obj(sstk_obj_t *obj)
{
    SWAP16(obj->dist_y);
    SWAP16(obj->dist_x);
    SWAP16(obj->speed_y);
    SWAP16(obj->headway_distance);
    SWAP16(obj->time_headway);
    SWAP16(obj->speed_x);
    SWAP16(obj->degree);
    SWAP32(obj->radar_x);
    SWAP32(obj->radar_y);
    SWAP64(*(uint64_t *)&obj->longitude);
    SWAP64(*(uint64_t *)&obj->latitude);
    SWAP32(obj->event_extra);
}

void fix_endian_sstk_lane(sstk_lane_t *lane)
{
    SWAP16(lane->queue_length);
}

void fix_endian_sstk_ll_data(sstk_ll_data_t *ll)
{
    SWAP16(ll->large_vehicle_count);
    SWAP16(ll->large_vehicle_average_speed);
    SWAP16(ll->medium_vehicle_count);
    SWAP16(ll->medium_vehicle_average_speed);
    SWAP16(ll->small_vehicle_count);
    SWAP16(ll->small_vehicle_average_speed);
    SWAP16(ll->non_motor_vehicle_count);
    SWAP16(ll->non_motor_vehicle_average_speed);
    SWAP16(ll->average_vehicle_speed);
    SWAP16(ll->lane_occupancy);
    SWAP16(ll->time_occupancy);
    SWAP16(ll->average_time_headway);
    SWAP16(ll->average_vehicle_spacing);
}

static void send_event_by_mb(sstk_obj_t *obj, uint16_t event_id, uint8_t dev_type)
{
    radar_packet_84_t packet_84 = {0};
    packet_84.header.event_type = event_id;
    packet_84.body.va.obj_id = obj->id;
    packet_84.body.va.obj_lane = obj->lane_id;
    packet_84.body.va.obj_type = convert_sstk_to_platform(obj->type);
    packet_84.body.va.obj_length = obj->length;
    packet_84.body.va.obj_dist_y_low = obj->dist_y;
    packet_84.body.va.obj_dist_x_low = obj->dist_x;
    packet_84.body.va.obj_speed = obj->speed_y;
    uint8_t time_stamp[8] = {0};
    send_event_to_platform(&packet_84, time_stamp, dev_type);
}

// 间隔时间
#define TIME_INTERVAL (10 * 1000)
bool update_event_queue_sstk(sstk_obj_t *obj, uint32_t event_id)
{
    int count = get_queue_count(g_event_manager.sstk_event_queue);
    for (int i = 0; i < count; i++) {
        sstk_event_static_t *event = queue_get_node(g_event_manager.sstk_event_queue, from_tail, i);
        if (event && event->obj_info.id == obj->id && event->event == event_id) {
            int64_t current_time = get_current_time_in_milliseconds();
            // 如果两次上报时间间隔大于TIME_INTERVAL秒，则认为是新的事件
            if ((current_time - event->last_report_time) > TIME_INTERVAL) {
                event->first_report_time = current_time;
                event->last_report_time = current_time;
                event->duration = 0;
                memcpy(&event->obj_info, obj, sizeof(sstk_obj_t));
                return true;
            }

            // 如果事件已经存在，则修改持续时间
            memcpy(&event->obj_info, obj, sizeof(sstk_obj_t));
            event->duration = (current_time - event->first_report_time);
            event->last_report_time = current_time;
            return false;
        }
    }

    // 如果事件不存在，则创建新的事件并添加到队列
    sstk_event_static_t new_event = {0};
    memcpy(&new_event.obj_info, obj, sizeof(sstk_obj_t));
    new_event.event = event_id;
    new_event.first_report_time = get_current_time_in_milliseconds();
    new_event.last_report_time = new_event.first_report_time;
    new_event.duration = 0;  // 初始持续时间为0
    queue_enqueue(g_event_manager.sstk_event_queue, &new_event);
    return true;
}

static void analysis_mb_event(sstk_obj_t *obj, uint8_t dev_type)
{
    uint16_t event_id = 0;
    if (IS_BIT_SET(obj->event, 0)) {
        // send_event_by_mb(obj, 0x01, dev_type);  // 异常停车
        event_id = 0x01;
    }
    if (IS_BIT_SET(obj->event_extra, 0) || IS_BIT_SET(obj->event_extra, 4)) {
        // send_event_by_mb(obj, 0x12, dev_type);  // 违法停车
        event_id = 0x12;
    }
    if (IS_BIT_SET(obj->event, 6)) {
        // send_event_by_mb(obj, 0x02, dev_type);  // 逆行
        event_id = 0x02;
    }
    if (IS_BIT_SET(obj->event, 2)) {
        // send_event_by_mb(obj, 0x03, dev_type);  // 超速
        event_id = 0x03;
    }
    if (IS_BIT_SET(obj->event, 7)) {
        // send_event_by_mb(obj, 0x4, dev_type);  // 变道
        event_id = 0x04;
    }
    if (IS_BIT_SET(obj->event, 5)) {
        // send_event_by_mb(obj, PEDESTRIAN_EVENT_CODE, dev_type);  // 行人
        // update_event_queue_sstk(obj, PEDESTRIAN_EVENT_CODE);  // 更新事件队列
        event_id = PEDESTRIAN_EVENT_CODE;
    }
    if (IS_BIT_SET(obj->event, 4)) {
        // send_event_by_mb(obj, 0x06, dev_type);  // 低速
        event_id = 0x06;
    }
    if (IS_BIT_SET(obj->event_extra, 2)) {
        // send_event_by_mb(obj, 0x07, dev_type);  // 未保持安全车距
        event_id = 0x07;
    }
    if (IS_BIT_SET(obj->event_extra, 1)) {
        // send_event_by_mb(obj, 0x11, dev_type);  // 占用应急车道
        event_id = 0x11;
    }
    if (IS_BIT_SET(obj->event_extra, 6)) {
        // send_event_by_mb(obj, 0xA, dev_type);  // 缓行事件
        // update_event_queue_sstk(obj, 0xA);       // 更新事件队列
        event_id = 0xA;
    }
    if (event_id != 0) {
        if (update_event_queue_sstk(obj, event_id)) {
            send_event_by_mb(obj, event_id, dev_type);
        }
    }
}

/**
 * @brief 解析并处理 MB 协议的雷达数据，将目标对象转化为标准 radar_frame_t 结构，
 *        并入队保存以供后续融合使用。
 *
 * @param buffer   原始 MB 协议数据（对象数组）
 * @param length   数据长度
 * @param jb       雷达帧附加信息（含目标数量、时间戳等）
 * @param dev_type 设备类型（RADAR_INCOMING 来向 or RADAR_OUTGOING 去向）
 */
static void analysis_mb(char *buffer, int length, sstk_jb_t *jb, uint8_t dev_type)
{
    radar_frame_t radar_frame = {0};  // 初始化标准雷达帧结构
    uint16_t obj_num = jb->obj_num;   // 当前帧的目标数量
    uint8_t i;
    char buf[512] = {0};
    char time_stamp[64] = {0};
    Bitmap obj_map = {0};  // 初始化该帧的目标位图（用于融合匹配）

    // 调试模式下打印时间戳
    if (g_config.radar_debug == 1) {
        printf("时间:%s\n", get_current_time_string(time_stamp, sizeof(time_stamp)));
    }

    sstk_obj_t *obj_info = (sstk_obj_t *)buffer;  // MB 协议对象数组指针

    for (i = 0; i < obj_num; i++) {
        sstk_obj_t *obj = &obj_info[i];
        fix_endian_sstk_obj(obj);                                   // 修正字节序，兼容大小端平台

        radar_obj_info_t radar_obj_info = {0};                      // 构造内部标准目标对象结构
        radar_obj_info.id = obj->id;
        radar_obj_info.lane = lane_to_index(obj->lane_id);          // 车道ID转换为内部索引
        radar_obj_info.type = convert_sstk_to_platform(obj->type);  // 类型转换
        radar_obj_info.length = obj->length;
        radar_obj_info.dist_y = obj->dist_y;
        radar_obj_info.dist_x = obj->dist_x;
        radar_obj_info.speed_y = obj->speed_y;
        radar_obj_info.speed_x = obj->speed_x;
        radar_obj_info.speed = obj->speed_y;  // 主速度方向设为 Y
        radar_obj_info.longitude = obj->longitude;
        radar_obj_info.latitude = obj->latitude;

        // 安全边界保护，防止数组越界
        if (radar_frame.obj_num >= RADAR_INFO_SIZE) {
            break;
        }
        update_new_enter_obj(&radar_obj_info, dev_type);  // 更新新进入目标状态

        // 分析事件（如碰撞、静止等）
        analysis_mb_event(obj, dev_type);

        // 打印调试日志：格式化输出每个目标信息（分来向/去向）
        if (dev_type == RADAR_INCOMING && radar_obj_info.lane != -1) {
            sprintf(buf, ">>>>>>>>>>>>>>>>>来向雷达ID: 0x%04X, 道路号: %d, 距离y: %d, 距离x: %d, 速度: %d,经度: %f, 纬度: %f, 高度: %d",
                    radar_obj_info.id, radar_obj_info.lane, radar_obj_info.dist_y, radar_obj_info.dist_x,
                    radar_obj_info.speed_y, radar_obj_info.longitude, radar_obj_info.latitude, radar_obj_info.height);
            if (g_config.radar_debug == 1) {
                printf("\033[32m%s\033[0m\n", buf);
            }
            write_log("测试接力", buf, NULL);
        }

        if (dev_type == RADAR_OUTGOING && radar_obj_info.lane != -1) {
            // if (dev_type == RADAR_OUTGOING) {
            sprintf(buf, "<<<<<<<<<<<<<<<<<去向雷达ID: 0x%04X, 道路号: %d, 距离y: %d, 距离x: %d, 速度: %d,经度: %f, 纬度: %f, 高度: %d",
                    radar_obj_info.id, radar_obj_info.lane, radar_obj_info.dist_y, radar_obj_info.dist_x,
                    radar_obj_info.speed_y, radar_obj_info.longitude, radar_obj_info.latitude, radar_obj_info.height);
            if (g_config.radar_debug == 1) {
                printf("\033[34m%s\033[0m\n", buf);
            }
            write_log("测试接力", buf, NULL);
        }

        // 跳过非法车道
        if (radar_obj_info.lane == -1) {
            continue;
        }

        // 根据来/去向，判断目标是否在设定距离范围内，并进行栅格映射
        if (dev_type == RADAR_INCOMING) {
            if (radar_obj_info.dist_y < g_config.in_radar_max_distance &&
                radar_obj_info.dist_y >= g_config.in_radar_min_distance + g_config.in_radar_offset) {
                // 将距离映射为 bitmap 列索引
                int bit_y = MAX_COLS * (radar_obj_info.dist_y - g_config.in_radar_min_distance) /
                            (g_config.in_radar_max_distance - g_config.in_radar_min_distance);

                set_bit(&obj_map, radar_obj_info.lane, bit_y);  // 在位图中设置该目标位置
                radar_obj_info.row = radar_obj_info.lane;
                radar_obj_info.col = bit_y;

                // 存入雷达帧对象列表
                memcpy(&radar_frame.obj_info[radar_frame.obj_num++], &radar_obj_info, sizeof(radar_obj_info_t));
            }
            else {
                // 超出距离范围，标记为无效位置
                radar_obj_info.row = -1;
                radar_obj_info.col = -1;
                memcpy(&radar_frame.obj_info[radar_frame.obj_num++], &radar_obj_info, sizeof(radar_obj_info_t));
            }
        }
        else {  // RADAR_OUTGOING
            if (radar_obj_info.dist_y < g_config.out_radar_max_distance &&
                radar_obj_info.dist_y >= g_config.out_radar_min_distance + g_config.out_radar_offset) {
                int bit_y = MAX_COLS * (radar_obj_info.dist_y - g_config.out_radar_min_distance) /
                            (g_config.out_radar_max_distance - g_config.out_radar_min_distance);
                set_bit(&obj_map, radar_obj_info.lane, bit_y);
                radar_obj_info.row = radar_obj_info.lane;
                radar_obj_info.col = bit_y;
                memcpy(&radar_frame.obj_info[radar_frame.obj_num++], &radar_obj_info, sizeof(radar_obj_info_t));
            }
            else {
                radar_obj_info.row = -1;
                radar_obj_info.col = -1;
                memcpy(&radar_frame.obj_info[radar_frame.obj_num++], &radar_obj_info, sizeof(radar_obj_info_t));
            }
        }
    }

    // char time_str[64];
    // printf("雷达位图: %s\n", get_current_time_string(time_str, sizeof(time_str)));
    // print_bitmap(&obj_map);

    // 设置帧时间和时间戳
    radar_frame.frame_time = get_current_time_in_milliseconds();
    memcpy(radar_frame.time_stamp, jb->time_stamp, sizeof(jb->time_stamp));
    memcpy(&radar_frame.obj_map, &obj_map, sizeof(Bitmap));

    // 将该帧入队到全局队列（按来/去向区分）
    if (dev_type == RADAR_INCOMING) {
        queue_enqueue(g_radar_manager.in_radar_frame_queue, &radar_frame);
        try_to_pole_relay(&radar_frame);  // 尝试接力传输
    }
    else {
        queue_enqueue(g_radar_manager.out_radar_frame_queue, &radar_frame);
        if (g_config.need_pole_relay) {
            try_to_pole_relay(&radar_frame);
        }
        else if (g_config.need_third_relay) {
            try_to_relay_by_third(&radar_frame);
        }
    }

    // 打印调试标记（可选）
    if (obj_num > 0 && g_config.radar_debug == 1) {
        write_log("测试接力", "=============================================\n", NULL);
    }
    if (g_config.radar_debug == 1) {
        printf("---------------------------------------------\n\n");
    }
}

void send_event_by_dl(sstk_lane_t *lane, uint8_t dev_type)
{
    radar_packet_84_t packet_84 = {0};
    packet_84.header.event_type = 0x43;
    packet_84.body.tj2.lane_num = lane->lane_id;
    packet_84.body.tj2.traffic_jam_length = lane->queue_length;
    packet_84.body.tj2.congestion_level = lane->queue_congestion_level;
    packet_84.body.tj2.is_overflow = lane->overflow_tag;
    uint8_t time_stamp[8] = {0};
    send_event_to_platform(&packet_84, time_stamp, dev_type);
}

void print_dl_info(sstk_lane_t *lane)
{
    printf("DL 信息: 道路号: %d, 队列长度: %d, 拥堵等级: %d, 溢出标志: %d\n",
           lane->lane_id, lane->queue_length, lane->queue_congestion_level, lane->overflow_tag);
}

static void analysis_dl(char *buffer, int length, sstk_jb_t *jb, uint8_t dev_type)
{
    // 处理 DL 协议数据
    sstk_lane_t *dl_data = (sstk_dl_t *)buffer;
    uint8_t lane_count = jb->lane_count;

    for (int i = 0; i < lane_count; i++) {
        sstk_lane_t *lane = &dl_data[i];
        fix_endian_sstk_lane(lane);
        if (event_lane_filter(lane->lane_id) == false) {
            continue;  // 道路号过滤
        }
        // print_dl_info(lane);               // 打印 DL 信息
        send_event_by_dl(lane, dev_type);  // 发送事件
    }
}

void print_ll_info(sstk_ll_data_t *ll)
{
    printf(
        "LL 信息: 道路号: %d, 大型车辆数量: %d, 大型车辆平均速度: %d, "
        "中型车辆数量: %d, 中型车辆平均速度: %d, "
        "小型车辆数量: %d, 小型车辆平均速度: %d, "
        "非机动车数量: %d, 非机动车平均速度: %d, "
        "平均车辆速度: %d, 车道占有率: %d, "
        "时间占有率: %d, 平均时间间隔: %d, 平均车辆间距: %d\n",
        ll->lane_id,
        ll->large_vehicle_count, ll->large_vehicle_average_speed,
        ll->medium_vehicle_count, ll->medium_vehicle_average_speed,
        ll->small_vehicle_count, ll->small_vehicle_average_speed,
        ll->non_motor_vehicle_count, ll->non_motor_vehicle_average_speed,
        ll->average_vehicle_speed,
        ll->lane_occupancy,
        ll->time_occupancy,
        ll->average_time_headway,
        ll->average_vehicle_spacing);
}

static void analysis_ll(char *buffer, int length, sstk_jb_t *jb, uint8_t dev_type)
{
    int max_entries = length / sizeof(sstk_ll_data_t);
    // 一次性分配 header + max_entries * body
    radar_packet_80_t *packet = malloc(sizeof(radar_packet_80_t) + max_entries * sizeof(data_80_body_t));
    if (!packet) {
        printf("Memory allocation failed for radar_packet_80_t\n");
        return;
    }

    packet->header.flow_cycle = jb->ll_report_interval;
    packet->header.lane_count = 0;

    int offset = 0;
    while (offset + sizeof(sstk_ll_data_t) <= length) {
        sstk_ll_data_t *ll = (sstk_ll_data_t *)(buffer + offset);
        fix_endian_sstk_ll_data(ll);

        if (event_lane_filter(ll->lane_id)) {
            print_ll_info(ll);
            data_80_body_t *body = &packet->body[packet->header.lane_count++];
            body->way_id = ll->lane_id;
            body->sum_vehicle = ll->large_vehicle_count + ll->medium_vehicle_count + ll->small_vehicle_count;
            body->a_flow_rate_big = ll->large_vehicle_count;
            body->b_flow_rate_mid = ll->medium_vehicle_count;
            body->c_flow_rate_small = ll->small_vehicle_count;
            body->avg_time_use_rate = ll->time_occupancy / 10;        // 转为百分比
            body->avg_flow_rate_vehicle = ll->average_vehicle_speed;  // m/s
            body->avg_length_vehicle = 0;
            body->avg_dis_time = ll->average_time_headway / 10;       // s
            body->avg_dis_inter = 0;
            body->statistic_85_Speed = ll->lane_occupancy / 10;       // 空间占有率%
            body->person_flow_rate = 0;
            body->flow_nonmotor = ll->non_motor_vehicle_count;
            body->reserve[0] = 0;
        }

        offset += sizeof(sstk_ll_data_t);
    }

    if (packet->header.lane_count > 0) {
        send_statistics_data(packet, dev_type);
    }
    free(packet);
}

// 分析 SSTK 协议的雷达数据包
void analysis_radar_buffer_sstk(char *buffer, int length, uint8_t dev_type)
{
    uint16_t offset = 0;
    uint16_t packet_length = 0;
    bool find_jb = false;

    buffer_push(&cb, buffer, length);
    int pkt_len = 0;
    uint8_t *pkt;

    while (pkt = buffer_parse(&cb, &pkt_len)) {
        // 处理 pkt_len 字节的数据包
        offset = 0;
        sstk_header_t *header = (sstk_header_t *)pkt;
        fix_endian_sstk_header(header);
        if (header->type != FRAM_HEAD) {
            printf("Not a radar packet\n");
            return;
        }
        if (header->length + 5 != pkt_len) {
            printf("Invalid packet length: expected %d, got %d\n", header->length + 5, pkt_len);
            return;
        }
        offset += sizeof(sstk_header_t);

        header = (sstk_header_t *)(pkt + offset);
        fix_endian_sstk_header(header);
        if (header->type != JB_HEAD) {
            printf("没有找到基本信息模块\n");
            return;
        }
        sstk_jb_t *jb_data = (sstk_jb_t *)(pkt + offset + sizeof(sstk_header_t));
        fix_endian_sstk_jb(jb_data);
        offset += header->length;

        while (offset < pkt_len - 1) {
            header = (sstk_header_t *)(pkt + offset);
            fix_endian_sstk_header(header);
            switch (header->type) {
                case MB_HEAD:
                    // printf("Found MB packet, length: %d\n", header->length);
                    analysis_mb(pkt + offset + sizeof(sstk_header_t), header->length - 5, jb_data, dev_type);
                    break;
                case DL_HEAD:
                    // printf("Found DL packet, length: %d\n", header->length);
                    analysis_dl(pkt + offset + sizeof(sstk_header_t), header->length - 5, jb_data, dev_type);
                    break;
                case LL_HEAD:
                    // printf("Found LL packet, length: %d\n", header->length);
                    analysis_ll(pkt + offset + sizeof(sstk_header_t), header->length - 5, jb_data, dev_type);
                    break;
                default:
                    printf("Unknown packet type: 0x%04X\n", header->type);
                    return;
            }
            offset += header->length;
        }
    }
}